package Behaviors.Schemas.Motor;
import Behaviors.Schemas.Perception.*;
import Behaviors.Schemas.Schema;
import Behaviors.Schemas.Vector2dSchema;
import Robot.Sensors.Sensor;
import Util.Vector2d;

import Robot.*;

public class MSAvoidObstacles extends Schema implements Vector2dSchema {
    
    //Perception Schema
    private PSDetectObstacles detect;
    private Position2d myPos;
    //maxRange deberia salir de las propiedades del sensor!!!
    private float maxRange=0.4f;
    private float exp_param = 2.0f;
    public static final int MODE_LINEAR = 0;
    public static final int MODE_EXPONENTIAL = 1;
    private int atractorMode;
    
    public MSAvoidObstacles(String name,Schema detect, Position2d myPos,int atractorMode) {

        super(name);
        this.detect=(PSDetectObstacles)detect;
        this.myPos = myPos;
        this.atractorMode = atractorMode;
        /*Class classOfdetect=detect.getClass();
        String nombre="Vector2dSchema";
        nombre.equals(classOfdetectclassOfdetect.getInterfaces()[0].getName());*/
        
    }

    public void setMaxRange(float maxRange)
    {
        this.maxRange = maxRange;
    }
    
    public float getMaxRange()
    {
        return maxRange;
    }
    
    public Vector2d Value() {
        
        //This is the sum of magnitudes of all vectors.
        float sumMag = 0;
        //this is the magnitude of the biggest vector
        float maxMag = 0;


        //Position of obstacles
        float myDir = myPos.read(Sensor.READ_MODE_NO_WAIT).getPa();
        //System.out.println("Pa of robot: "+Float.toString(myDir));
        Vector2d[] detectValue=detect.Value();
        
        //Convert All vectors in attractors
        //Just Sum all vectors. The inverse of result is the vector used to avoid
        //obstacles.
        Vector2d total=new Vector2d(0f,0f,0);
        
        for(int i=0;i<detectValue.length;i++){
            
            if(detectValue[i].getMag()>0){
                    
                //Obstacles are globally localized.
                //Reference them to the robots point of view
                detectValue[i] = detectValue[i].sub(myPos.getPositionVector());
                detectValue[i].setAngle(detectValue[i].getAngle() - myDir);

                //0.15f es el radio del robot!!!
                detectValue[i].setMag(detectValue[i].getMag()-0.15f);

                switch(atractorMode){

                    case MODE_LINEAR:
                        detectValue[i] = convertLinearMode(detectValue[i]);
                        break;
                    case MODE_EXPONENTIAL:
                        detectValue[i] = convertExponentialMode(detectValue[i]);
                        break;
                }
                //now the vector has a magnitude in the range of [0, 1]
                float actualMag = detectValue[i].getMag();

                //System.out.println("Obstacle No. "+ i);
                //detectValue[i].print(Vector2d.MODE_MA);
                //sum magnitude of all vectors
                sumMag += actualMag;

                //search for the biggest vector of the array
                if(maxMag < actualMag)
                    maxMag = actualMag;


            }
            if(i==0)
                total = detectValue[i];
            else
            { 
                //Sum current vector with the last one, and sum it to total
                total=total.add(detectValue[i]);
            }
                
            
        }
        total = total.rotate(myDir);

        //System.out.println("Magnitud de vector: "+total.getMag());

        //System.out.println("SumMag = "+sumMag);
        if(sumMag == 0  && maxMag == 0)
                maxMag = sumMag = 1;
        //total = total.normalize();
        total = total.scale(-maxMag/sumMag);
        //Correction to coordinates of map
        
      //  total.setMag(0);
        //System.out.println("Mag: "+Float.toString(total.getMag())+" theta: "+Double.toString(Math.toDegrees(total.getTheta())));
        
        if(enabled)
            return total;
        else
            return new Vector2d();
    }
    
    
    private Vector2d convertExponentialMode(Vector2d atractor){
        
        float x = atractor.getMag();
        
        //if object is in the closest area, then shutdown atractor
        if(x<maxRange)
        {
            float newMag =(float)(1 - Math.exp(-(maxRange-x)/exp_param));

            atractor.setMag(newMag);
            
        
        }
        else
            atractor.setMag(0);
        
        return atractor;
    
    }
    
    
    private Vector2d convertLinearMode(Vector2d atractor){

        //if obstacle in area of influence
        if(atractor.getMag()<maxRange){

            //System.out.println("Obst No. "+i+" is in influence area!");
            float mag = (maxRange - atractor.getMag())/maxRange;
            atractor.setMag(mag);

        }
        else
        {
            //System.out.println("Obst No. "+i+" is NOT in influence area!");
            atractor.setMag(0);
        }
        
        return atractor;

    
    }

    //distancia a la cual el vector de velocidad se encuentra al 63% (0.63).
    public void setExponentialDistance(float x){
        
        //exp_param = (float)-Math.log(1 - Math.sqrt(2)/2.0f) / x;
        exp_param = (float)maxRange-x;

    
    }
}
